package estrategia;

import java.io.IOException;
import java.util.ArrayList;
import java.util.Collection;
import java.util.Collections;
import java.util.Iterator;
import java.util.List;

import javax.vecmath.Point3d;

import protocolo.AliadoDistancia;
import protocolo.Atendedor;
import protocolo.Enviador;
import protocolo.Pedido;
import protocolo.Pedido.TipoPedido;
import protocolo.Pedido.TipoTarea;

import monitor.DetectorAtascamientoD;
import monitor.ManagerObjetos;
import CamposDePotencial.EstrategiaCampoPotencial;
import CamposDePotencial.Objetivo;

import comunicacion.Conector;
import comunicacion.ParametrosEntorno;
import comunicacion.comandos.Commands;
import comunicacion.comandos.EntornoReal;
import comunicacion.simulador.Simulador;


public class Controlador {	
	public Conector entorno;
	
	public Predictor predictor;
	public ManagerObjetos managerObjetos;
	
	private Utiles utiles;
	private DetectorAtascamientoD monitorAtascamiento;
	
	private Deliberador deliberador;
	
	private long instanteInicialPartida;
	private long instanteInicialTimerAccepted;
	private long instanteInicialTimerOcupados;
	
	public boolean puedo_usar_motores = true; //Bandera para mutuo excluir el uso de los motores entre Controlador y DetectorAtascamientoD
	
	public enum Estado {
		NO_COOPERANDO, ESPERANDO_ACCEPT, COOPERANDO, EVALUANDO_OFERTAS
	} 
	
	private Boolean TERMINE_COOPERAR;
	
	private Estado estado;

	private Boolean estoyDefendiendoAtaque = new Boolean(false);
	private int enemigoAtacante = -1;
	
	public Controlador(int entornoConexion) throws IOException {		
		// Inicializamos los parametros de configuracion del entorno
		ParametrosEntorno.cargarPropiedades(entornoConexion);
		
		switch (entornoConexion) {
			case ParametrosEntorno.CONECTAR_CON_SIMULADOR:
				entorno = new Simulador(ParametrosGlobales.simIP, ParametrosGlobales.simPuerto, 0, 1);
				break;
			case ParametrosEntorno.CONECTAR_CON_ENTORNO_REAL:
				entorno = new EntornoReal(ParametrosGlobales.comandosIP, ParametrosGlobales.comandosPuerto, ParametrosGlobales.macRobot, Commands.NXT_MOTOR_A, Commands.NXT_MOTOR_B);
				break;
			default:
				entorno = null;	
		}
		
		managerObjetos = ManagerObjetos.getInstance(entorno);
		
		utiles = new Utiles(entorno);
		predictor = new Predictor(entorno, managerObjetos);
				
		deliberador = new Deliberador(entorno, managerObjetos);
		
		monitorAtascamiento = new DetectorAtascamientoD(this);
		monitorAtascamiento.start();		
	}
	
	public void moverRobotLyapunov(Point3d destino) throws IOException {
		Point3d actual = entorno.getCoordenadas(); 
		double distancia = utiles.distanciaEuclidea(actual, destino);
		
		while (distancia > ParametrosEntorno.DISTANCIA_ACEPTABLE) {			
			double v = utiles.velocidadLinealLV(actual, destino);				
			double w = utiles.velocidadAngularLV(actual, destino);
			
			double vl = v - w*ParametrosGlobales.DISTANCIA_ENTRE_RUEDAS/2.0;
			double vr = v + w*ParametrosGlobales.DISTANCIA_ENTRE_RUEDAS/2.0;
			
			entorno.enviarVelocidades(vl, vr);
			
			actual = entorno.getCoordenadas();
			distancia = utiles.distanciaEuclidea(actual, destino);
			
		}
		
		entorno.detenerMotores();
	}
	
	public void moverRobotDirect(Point3d destino) throws IOException {
		
		Point3d actual = entorno.getCoordenadas(); 
		double distancia = utiles.distanciaEuclidea(actual, destino);
		
		double maxVelocidad = ParametrosEntorno.MAXIMA_VELOCIDAD_DIRECT;
		double factorVariacion = ParametrosEntorno.FACTOR_VARIACION_VELOCIDAD_DIRECT;
		
		while (distancia > ParametrosEntorno.DISTANCIA_ACEPTABLE && puedo_usar_motores) {	
			// Obtenemos el angulo que forma el punto actual con respecto al destino
			double theta = utiles.getAnguloTheta(actual, destino);
			double alpha = utiles.getAnguloAlpha(theta, actual.z);
			
//			double vl, vr;
//			if (alpha < 0) {
//				vl = maxVelocidad;
//				vr = maxVelocidad + factorVariacion*alpha;
//			} else {
//				vl = maxVelocidad - factorVariacion*alpha;
//				vr = maxVelocidad;
//			}
			
			double vl, vr;
			if (Math.abs(alpha) <= Math.PI/2) {
				if (alpha < 0) {
					vl = maxVelocidad;
					vr = maxVelocidad + factorVariacion*alpha;
				} else {
					vl = maxVelocidad - factorVariacion*alpha;
					vr = maxVelocidad;
				}
			} else {
				// Aca agregamos DirectAng
				if (alpha < 0) {
					vl = -maxVelocidad;
					vr = -maxVelocidad + factorVariacion*(Math.PI+alpha);
				} else {
					vl = -maxVelocidad + factorVariacion*(Math.PI-alpha);
					vr = -maxVelocidad;
				}
			}
			
			entorno.enviarVelocidades(vl, vr);
			
			try {
				Thread.sleep(40);
			} catch (InterruptedException e) {}
			
			actual = entorno.getCoordenadas();
			distancia = utiles.distanciaEuclidea(actual, destino);
		}
		
		if( puedo_usar_motores ) {
			entorno.detenerMotores();
		}
	}
	
	public void moverRobotInterceptarEnemigo(int idEnemigo) throws IOException {
		
		Point3d actual = entorno.getCoordenadas();
		Point3d destino = entorno.getCoordenadas(idEnemigo);		
		double distancia = utiles.distanciaEuclidea(actual, destino);		
		double RELACION_DISTANCIA_PASOS_HACIA_FUTURO = 20;
		
		double maxVelocidad = ParametrosEntorno.MAXIMA_VELOCIDAD_DIRECT;
		double factorVariacion = ParametrosEntorno.FACTOR_VARIACION_VELOCIDAD_DIRECT;
		
		while (distancia > 2*ParametrosEntorno.DISTANCIA_ACEPTABLE) {	
			// Obtenemos el angulo que forma el punto actual con respecto al destino
			double theta = utiles.getAnguloTheta(actual, destino);
			double alpha = utiles.getAnguloAlpha(theta, actual.z);
			
			double vl, vr;
			if (alpha < 0) {
				vl = maxVelocidad;
				vr = maxVelocidad + factorVariacion*alpha;
			} else {
				vl = maxVelocidad - factorVariacion*alpha;
				vr = maxVelocidad;
			}
			
			entorno.enviarVelocidades(vl, vr);
			
			try {
				Thread.sleep(40);
			} catch (InterruptedException e) {}
			
			actual = entorno.getCoordenadas();
			Point3d enemigo = entorno.getCoordenadas(idEnemigo);
			distancia = utiles.distanciaEuclidea(actual, enemigo);
			int pasosHaciaFuturo = (int) (distancia * RELACION_DISTANCIA_PASOS_HACIA_FUTURO);
			destino = predictor.predecirPosicion(idEnemigo, pasosHaciaFuturo);		
		}
		
		entorno.detenerMotores();
		System.out.println("Te atrape elemento " + idEnemigo);
	}
	
	public void moverRobotMixto(Point3d destino) throws IOException {
		Point3d actual = entorno.getCoordenadas();
		int cuadrante = utiles.getCuadrante(actual, destino);
		
		double DISTANCIA_LYAPUNOV_DIRECT = 0.5;
		
		switch (cuadrante) {
			case 1: 
				if (destino.x > DISTANCIA_LYAPUNOV_DIRECT && 
						1.79 - destino.y > DISTANCIA_LYAPUNOV_DIRECT) {						
					double margen = 2.0 * DISTANCIA_LYAPUNOV_DIRECT / 3.0;
					Point3d intermedio;
					intermedio = new Point3d(destino.x - margen, destino.y + margen+0.05, 0);
					moverRobotDirect(intermedio);
					moverRobotLyapunov(destino);
				} else {
					moverRobotDirect(destino);
				}							
				break;
			case 4:
				if (destino.x > DISTANCIA_LYAPUNOV_DIRECT &&						
						destino.y > DISTANCIA_LYAPUNOV_DIRECT) {
					double margen = 2.0 * DISTANCIA_LYAPUNOV_DIRECT / 3.0;
					Point3d intermedio;
					intermedio = new Point3d(destino.x - margen, destino.y - margen-0.05, 0);
					moverRobotDirect(intermedio);
					moverRobotLyapunov(destino);
				} else {
					moverRobotDirect(destino);
				}							
				break;
			case 2: case 3:
				moverRobotLyapunov(destino);
		}
	}	
	
	public void elegirPuntoOptimoBorrarEsto() throws IOException {
		Deliberador deliberador = new Deliberador(entorno, managerObjetos);
		
		Point3d punto = deliberador.calcularPuntoOptimo(managerObjetos.getFuertes());
		
		System.out.println("Punto Optimo elegido = (" + punto.x + ", " + punto.y + ", " + punto.z + ")");
	}
	
	public Point3d[] getPosicionesObstaculos() throws IOException {
		
		return entorno.getCoordenadasObjetos();
	}
	
	public Point3d getMiPosicion() throws IOException {
		return entorno.getCoordenadas();
	}
	
	
	/**
	 * Detiene los motores, sin considerar si tiene o no el control de ellos.
	 * */
	public void detenerMotores() throws IOException {
		
		entorno.detenerMotores();
	}
	
	/**
	 * Girar hacia destino, sin considerar si tiene o no el control de los motores
	 * */
	public void girar(Point3d destino) {
		try {
			Point3d actual = entorno.getCoordenadas();
			double theta = utiles.getAnguloTheta(actual, destino);
			
			girar(theta);
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}
	
	
	/**
	 * Girar theta, sin considerar si tiene o no el control de los motores
	 * */
	public void girar(double theta) {
	
		if (theta > 2*Math.PI) {
			theta = theta % 2*Math.PI;
		}
		
		if (theta > Math.PI) {
			theta = theta - 2*Math.PI;
		}
			
		try {
			Point3d actual = entorno.getCoordenadas();
		
			double diferencia = utiles.getDiferenciaAngulos(theta, actual.z);
			
			//System.out.println("Diferencia = " + diferencia);
			
			int izq = 1;
			if (theta >=0 && actual.z >=0 || theta <=0 && actual.z <=0) {
				if (theta <= actual.z) {
					izq = 1;
				} else {
					izq = -1;
				}
			} else {
				if (actual.z <= 0 && theta >= 0) {
					if (actual.z + Math.PI >= theta) {
						izq = -1;
					} else {
						izq = 1;
					}
				} else {
					if (actual.z - Math.PI <= theta) {
						izq = 1;
					} else {
						izq = -1;
					}
				}
			}
			
			/*
			while ( diferencia > ParametrosEntorno.TOLERANCIA_ANGULO) {
			//while(!Salird.angulosIguales(theta, actual.z)) {
				entorno.enviarVelocidades(izq*(Math.sqrt(diferencia)), -izq*(Math.sqrt(diferencia)));
				actual = entorno.getCoordenadas();
				diferencia = utiles.getDiferenciaAngulos(theta, actual.z);
				//diferencia = Salird.angulosIguales(theta, actual.z);
				//System.out.println("Diferencia78 = " + diferencia);
			}
			*/
			
			double angulo_barrido = 0;
			double angulo_barrido_total = utiles.getDiferenciaAngulos(theta, actual.z);
			
			//System.out.println("angulo_barrido_total = " + angulo_barrido_total);			
			while(angulo_barrido < angulo_barrido_total - ParametrosEntorno.MARGEN_GIRO  ) {
				double angulo_anterior = actual.z;
				entorno.enviarVelocidades(izq*(Math.sqrt(diferencia)), -izq*(Math.sqrt(diferencia)));
				actual = entorno.getCoordenadas();					
				angulo_barrido = angulo_barrido +  utiles.getDiferenciaAngulos(angulo_anterior, actual.z);
				//System.out.println("angulo_barrido = " + angulo_barrido);
			}
			
			entorno.detenerMotores();
			
		} catch (IOException e) {
			// TODO Auto-generated catch block
			System.out.println("Error al girar");
			e.printStackTrace();
		}
	}
	
	/**
	 * Retrocede a velocidad velocidad, sin controlar si tiene o no el control de los motores
	 * */
	public void retroceder(double velocidad) throws IOException {
		
		entorno.enviarVelocidades(-velocidad, -velocidad);	
		
	}
	
	/**
	 * Avanza a velocidad velocidad, sin controlar si tiene o no el control de los motores
	 * */
	public void avanzar(double velocidad) throws IOException {
		
		entorno.enviarVelocidades(velocidad, velocidad);	
		
	}
	
	public void camposDePotencial(Point3d objetivo) throws IOException {
		
		CamposPotencial camposPotencial = new CamposPotencial();
		Point3d mipos = entorno.getCoordenadas();
		Point3d proxPunto = mipos;
		
		while (utiles.distanciaEuclidea(proxPunto, objetivo) > ParametrosEntorno.DISTANCIA_ACEPTABLE) {
			mipos = entorno.getCoordenadas();
			Point3d[] obstaculos = getPosicionesObstaculos();
			proxPunto = camposPotencial.hallarProximoDestino(mipos, objetivo, obstaculos);
			
			girar(proxPunto);
			moverRobotDirect(proxPunto);		
		}	
	}	
	
	
	public void camposPotencialUnivectorial(Point3d g, Point3d r) throws IOException {
			CamposPotencialUnivectorial campos = new CamposPotencialUnivectorial(utiles);
			
			Point3d p = entorno.getCoordenadas();
			
			double Kp = 20;
			double Kd = 40;
			
			double theta_error_anterior = 0;
			double distancia = utiles.distanciaEuclidea(p, g);
			
			while (distancia > ParametrosEntorno.DISTANCIA_ACEPTABLE) {
			
				Point3d[] obstaculos = entorno.getCoordenadasObjetos();
				Vector resultante = campos.hallarDireccionCampo(p, g, r, obstaculos);
				
				double theta_error = resultante.direccion - p.z; 
				
				double theta_error_der = theta_error - theta_error_anterior;
				theta_error_anterior = theta_error;
				
				double w = Kp*theta_error + Kd*theta_error_der;
				
				double v = utiles.velocidadLinealLV(p, g);
				double Vr = v + (ParametrosGlobales.DISTANCIA_ENTRE_RUEDAS/2)*w;
				double Vl = v - (ParametrosGlobales.DISTANCIA_ENTRE_RUEDAS/2)*w;
				
				entorno.enviarVelocidades(Vl, Vr);
				
				p = entorno.getCoordenadas();
				distancia = utiles.distanciaEuclidea(p, g);
				
			}
			entorno.detenerMotores();
		}
	
	
	// Devuelvo true en caso de que pueda llegar al destino
	public boolean puedoLlegarAlDestino(Objetivo objetivo) throws IOException {
		boolean puedoLlegar = true;
		double distanciaMaxima = ParametrosPotenciales.DISTANCIA_FUERTE;
		// Si el objetivo es un punto y hay un robot cerca entonces devuelvo false
		if (! objetivo.esObjeto()) {
			Point3d posObjetivo = posObjetivo = objetivo.getPosicionObjetivo();;		
			Iterator<Integer> iterObjeto = managerObjetos.getObjetosActivos().iterator();
			
			while(iterObjeto.hasNext()) {
				int idObjeto = iterObjeto.next();
				Point3d posObjeto = predictor.predecirPosicion(idObjeto);
				if (objetivo.getIdObjetivo() != idObjeto) {
					if (utiles.distanciaEuclidea(posObjetivo, posObjeto) < distanciaMaxima) {
						puedoLlegar = false;
						break;
					}
				}
			}
			
		}
		
		return puedoLlegar;
	}
	
	// Realiza la invocacion al control campos de potencial creado un objetivo asociado a un objeto
	public void controlCamposPotencial(int idObjetivo) throws IOException {
		Objetivo objetivo = new Objetivo(idObjetivo, managerObjetos.esFuerte(idObjetivo), predictor);
		controlCamposPotencial(objetivo);
	}
	
	// Realiza la invocacion al control campos de potencial creado un objetivo puntual
	public void controlCamposPotencial(Point3d puntoObjetivo) throws IOException {
		Objetivo objetivo = new Objetivo(puntoObjetivo, predictor);
		controlCamposPotencial(objetivo);
	}
	
	// Control de movimiento asociado a una planificacio nde trayectorias con campos de potenciales
	public void controlCamposPotencial(Objetivo objetivo) throws IOException {
	
		EstrategiaCampoPotencial estrategia = new EstrategiaCampoPotencial(objetivo, managerObjetos.getObjetosActivos(), utiles, entorno, predictor);
		
		double maxVelocidad = ParametrosEntorno.MAXIMA_VELOCIDAD_DIRECT;
		double factorVariacion = ParametrosEntorno.FACTOR_VARIACION_VELOCIDAD_DIRECT;
		double distanciaAceptable = 0;
					
		if (objetivo.esObjeto()) {
			if (managerObjetos.esEjercito(objetivo.getIdObjetivo())) {
				if (managerObjetos.esEnemigo(objetivo.getIdObjetivo()))
					distanciaAceptable = ParametrosPotenciales.DISTANCIA_ENEMIGO;
				else 
					distanciaAceptable = ParametrosPotenciales.DISTANCIA_ALIADO;
			}else 
				distanciaAceptable = ParametrosPotenciales.DISTANCIA_FUERTE;
		} else {
			distanciaAceptable = ParametrosPotenciales.DISTANCIA_PUNTO;
		}
			
		Point3d puntoObjetivo = objetivo.getPosicionObjetivo();
		Point3d mipos = entorno.getCoordenadas();
		double distancia = utiles.distanciaEuclidea(mipos, puntoObjetivo);
		
		// || ! puedoLlegarAlDestino(objetivo)
		// && segundosRestantes() > 0
		while (distancia > distanciaAceptable && puedo_usar_motores && segundosRestantes() > 0) {
			// Le pido a la estrategia el destino dada mi posiciòn actual y el objetivo
			Point3d destino = estrategia.getDestino(mipos, puntoObjetivo);
			
			// Obtenemos el angulo que forma el punto actual con respecto al destino
			double theta = utiles.getAnguloTheta(mipos, destino);
			double alpha = utiles.getAnguloAlpha(theta, mipos.z);
			
			double vl, vr;
			if (alpha < 0) {
				vl = maxVelocidad;
				vr = maxVelocidad + factorVariacion*alpha;
			} else {
				vl = maxVelocidad - factorVariacion*alpha;
				vr = maxVelocidad;
			}
			
			/*double vl, vr;
			if (Math.abs(alpha) <= (Math.PI/2.0)) {
				if (alpha < 0) {
					vl = maxVelocidad;
					vr = maxVelocidad + factorVariacion*alpha;
				} else {
					vl = maxVelocidad - factorVariacion*alpha;
					vr = maxVelocidad;
				}
			} else {
				// Aca agregamos DirectAng
				if (alpha < 0) {
					vl = -maxVelocidad;
					vr = -maxVelocidad + factorVariacion*(Math.PI+alpha);
				} else {
					vl = -maxVelocidad + factorVariacion*(Math.PI-alpha);
					vr = -maxVelocidad;
				}
			}*/
		
			entorno.enviarVelocidades(vl, vr);
			
			try {
				Thread.sleep(40);
			} catch (InterruptedException e) {}
			
			mipos = entorno.getCoordenadas();
			if ( objetivo.esObjeto())
				puntoObjetivo = entorno.getCoordenadas(objetivo.getIdObjetivo());
			else
			    puntoObjetivo =objetivo.getPosicionObjetivo();
			distancia = utiles.distanciaEuclidea(mipos, puntoObjetivo);

		}
		
		if( puedo_usar_motores) {
			entorno.detenerMotores();
		}
		
		
		/*
		if (segundosRestantes() <= 0)
			System.out.println("FIN RECORRIDO POTENCIAL por tiempo");
		if (! puedo_usar_motores)
			System.out.println("FIN RECORRIDO POTENCIAL por atascamiento");
		else 
			System.out.println("FIN RECORRIDO POTENCIAL porque llegue");
		*/			
	}
	
		
	private long segundosRestantes() {
		long transcurrido = System.currentTimeMillis() - instanteInicialPartida;
		return (ParametrosGlobales.TIEMPO_JUEGO - (transcurrido / 1000));
	}
	
	private boolean timeOutAccepted() {
		long transcurrido = System.currentTimeMillis() - instanteInicialTimerAccepted;
		return (ParametrosGlobales.TIMEOUT_ESPERA_ACCEPTED <= (transcurrido / 1000));
	}
	
	private boolean timeOutOcupados() {
		long transcurrido = System.currentTimeMillis() - instanteInicialTimerOcupados;
		return (ParametrosGlobales.TIMEOUT_ALIADOS_OCUPADOS <= (transcurrido / 1000));
	}
	/**
	 * Es el metodo que efectivamente se encarga de ejecutar casi toda la estrategia de juego
	 * La idea es decidir y ejecutar una accion. cuando retoma el control, se repite el ciclo
	 */
	public void jugarConquistaDeFuertes() throws IOException {
		System.out.println("************ NUEVA BATALLA ************");
		System.out.println("Tiempo Total de Juego :: " + ParametrosGlobales.TIEMPO_JUEGO + " segundos");
		System.out.println("************ PARTICIPANTES ************");
		managerObjetos.imprimirClasificacionesActuales();
		
		instanteInicialPartida = System.currentTimeMillis();
		
		// Creamos los threads de envio y recepcion
		Enviador enviador = new Enviador();
		Atendedor atendedor = new Atendedor();
		enviador.start();
		atendedor.start();
		// inicializamos el estado
		estado = Estado.NO_COOPERANDO;
		TERMINE_COOPERAR = true;
			
		Paralelizador task = null;
		
		Pedido pedidoAyuda = null;
		
		while (segundosRestantes() > 0 && DetectorAtascamientoD.robotVivo()) {
			
			System.out.println("-------------------------------------------------");
			
			if (estado == Estado.NO_COOPERANDO) {
				
				System.out.println("ESTADO = NO COOPERANDO");
				
				synchronized (atendedor.pedidosAliados) {
					if (atendedor.pedidosAliados.isEmpty()) {				
						// Evaluo si tengo que pedir ayuda
						// PedirAyuda devuelve un pedido con tipo HELPME si hay que pedirAyuda
						Pedido pedido = pedirAyuda();
						if (pedido.esAyuda()) {
							
							pedidoAyuda = pedido;
							synchronized (TERMINE_COOPERAR) {
								if (! TERMINE_COOPERAR) {
									// esto es porque puedo estar ejecutando una tarea para la cual pedi ayuda
									// pero nadie me contesto entonces no estoy cooperando
									// y en este caso debo parar la funcion ejecutar tarea y mandar una nueva
									TERMINE_COOPERAR = true;
									// mandar un STOP
									//pararEjecucionTarea(task);
									task.stop();
								} 
							}
							// dejamos el pedido guardado en una variable para que lo pueda usar 
							// en el estado EvaluarOfertas y asi poder ejecutarTarea
							estado = Estado.EVALUANDO_OFERTAS;
							// Si tengo que pedir ayuda encolo el pedido en el buffer del enviador
							synchronized (enviador.pedidos) {
								enviador.pedidos.add(pedido);
							}
							
							// Yo me pongo a hacer la tarea y no espero a que alguien me responda que puede
							//ejecutarTarea(pedido);
							task =  new Paralelizador(this, pedido);	
							task.start();
							
							// Lanzo un timer para evitar quedarme esperando por respuestas de aliados
							instanteInicialTimerOcupados = System.currentTimeMillis();
						} else {
							synchronized (TERMINE_COOPERAR) {
								if (TERMINE_COOPERAR) {
									// esto es porque puedo estar ejecutando una tarea para la cual pedi ayuda
									// pero nadie me contesto entonces no estoy cooperando
									// y en este caso debo evaluar si ya termine de hacer esa tarea
									// si no termine la dejo que siga ejecutando
									// si termine ejecuto la estrategia
									
									// Si no tengo que pedir ayuda y el buffer de pedidos de los aliados esta vacio
									// ejecuto la estrategia normalmente
								
									// **** EjecutarEstrategia ****
									task =  new Paralelizador(this);	
									task.start();
								} 
							}
							
						}
					} else {
						// el buffer de pedidos de aliados no esta vacio
						Pedido pedidoInicial = atendedor.pedidosAliados.get(0);
						if (pedidoInicial.esAyuda()) {
							if (defendiendoAtaque()) {
								// Hay un pedido de ayuda y estoy defendiendome de un ataque
								// Entonces digo que estoy ocupado
								atendedor.pedidosAliados.remove(0);
								Pedido p = new Pedido(TipoPedido.IMBUSY, TipoTarea.NINGUNA, 0, "0", 0);
								synchronized (enviador.pedidos) {
									enviador.pedidos.add(p);
								}
								
								if (task.isAlive()) {
									task.stop();
								}
								// **** EjecutarEstrategia ****
								//new Paralelizador(this).start();
								task =  new Paralelizador(this);	
								task.start();
								
							} else {
								// Hay un pedido de ayuda y no estoy defendiendome de un ataque
								// Entonces digo que puedo
								Pedido p = new Pedido(TipoPedido.ICAN, TipoTarea.NINGUNA, 0, "0", 0);
								synchronized (enviador.pedidos) {
									enviador.pedidos.add(p);
								}
								// Ponemos a correr un timer para esperar un tiempo finito por un accept
								instanteInicialTimerAccepted = System.currentTimeMillis();
								// Actualizamos el estado
								estado = Estado.ESPERANDO_ACCEPT;
								
								if (task.isAlive()) {
									task.stop();
								}
								// **** EjecutarEstrategia ****
								//new Paralelizador(this).start();
								task =  new Paralelizador(this);	
								task.start();
							}
						}
					}
				}					
			}  else if (estado == Estado.ESPERANDO_ACCEPT) {
				
				System.out.println("ESTADO = ESPERANDO_ACCEPT");
				
				// Obtenemos el primer pedido del buffer
				Pedido pedidoInicial;
				synchronized (atendedor.pedidosAliados) {
					 pedidoInicial = atendedor.pedidosAliados.get(0);
				}
		
				if (pedidoInicial.esAccepted()) {
					// Si es Accepted entonces dejo todo lo que estaba haciendo y paso a cooperar
					detenerMotores();
					synchronized (atendedor.pedidosAliados) {
						atendedor.pedidosAliados.removeAll(atendedor.pedidosAliados);
						if (! atendedor.pedidosAliados.isEmpty())
							System.out.println("Error al vaciar el buffer de pedidos de aliados");
					}
					// cambio el estado a cooperando
					estado = Estado.COOPERANDO;
					// y paso a ejecutar la tarea
					// esta funcion debe cambiar el valor de la variable termine cooperar a true
										
					if (task.isAlive()) {
						task.stop();
					}
					
					//ejecutarTarea(pedidoInicial);
					//new Paralelizador(this, pedidoInicial).start();
					task =  new Paralelizador(this, pedidoInicial);	
					task.start();
				} else if (timeOutAccepted()){
					// elimino el pedido ese del buffer
					synchronized (atendedor.pedidosAliados) {
						atendedor.pedidosAliados.remove(0);
					}
					// Si no tengo un accepted en el primer lugar del buffer y expiro el tiempo de espera
					// volvemos al estado no cooperando
					estado = Estado.NO_COOPERANDO;
				}
				
			} else if (estado == Estado.COOPERANDO) {
				
				System.out.println("ESTADO = COOPERANDO");
				
				synchronized (atendedor.pedidosAliados) {
					if (! atendedor.pedidosAliados.isEmpty()) {
						// Obtenemos el primer pedido del buffer
						Pedido pedidoInicial = atendedor.pedidosAliados.remove(0);
						// Si es ayuda contestamos BUSY porque estamos cooperando
						if (pedidoInicial.esAyuda()) {
							Pedido p = new Pedido(TipoPedido.IMBUSY, TipoTarea.NINGUNA, 0, "0", 0);
							synchronized (enviador.pedidos) {
								enviador.pedidos.add(p);
							}
						}
						
					}
				}
				
				synchronized (TERMINE_COOPERAR) {
					if (TERMINE_COOPERAR)
						estado = Estado.NO_COOPERANDO;
				}
				
			} else if (estado == Estado.EVALUANDO_OFERTAS) {
				
				System.out.println("ESTADO = EVALUANDO_OFERTAS");
				
				// obtengo la posicion del objetivo
				//int idObjetivo = enviador.mapearIPAObjeto(pedidoAyuda.getDireccionIP());
				Point3d posObjetivo = entorno.getCoordenadas(pedidoAyuda.getIdObjetivo());
				
				// Devuelve NULL en caso de que no haya ningun ICAN optimo
				// Sino devuelve el pedido con el ican optimo con el tipo
				Pedido respuesta = obtenerICanOptimo(atendedor, enviador, posObjetivo);
				if (respuesta != null) {
					// Si encontramos un ICAN optimo en el buffer de recepcion de respuestas
					// Borramos el buffer de recepcion de respuestas
					synchronized (atendedor.respuestasAliados) {
						atendedor.respuestasAliados.removeAll(atendedor.respuestasAliados);
					}
					respuesta.setTipo(TipoPedido.ACCEPTED);
					synchronized (enviador.pedidos) {
						enviador.pedidos.add(respuesta);
					}
					
					estado = Estado.COOPERANDO;
					
					System.err.println("Encontre ICAN optimo");
				} else if (timeOutOcupados()) {
					respuesta = obtenerAliadoICanMasCercano(atendedor, enviador, posObjetivo);
					
					// Si dio timeout vacio el buffer y resuelvo que hacer
					synchronized (atendedor.respuestasAliados) {
						atendedor.respuestasAliados.removeAll(atendedor.respuestasAliados);
					}
					
					if (respuesta == null) {
						
						// Si dio timeout de ocupados pasamos al estado NO COOPERANDO
						estado = Estado.NO_COOPERANDO;
						
						System.err.println("Me dio timeout ocupados y no tengo ningun ICAN");
						
					} else {
						respuesta.setTipo(TipoPedido.ACCEPTED);
						synchronized (enviador.pedidos) {
							enviador.pedidos.add(respuesta);
						}
						
						estado = Estado.COOPERANDO;
						
						System.err.println("No encontre ICAN optimo pero me dio timeout y elegi el mejor de los icanes");
					}
				}	
			} else {
				System.err.println("Estoy en un estado erroneo");
			}
			
			try {
				Thread.sleep(1000);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
			
		}
		
		
		// <BORRAR>
		System.out.println("Fin Juego");
		int resultadoFinal = deliberador.getFuertesConquistables(-1, entorno.getCoordenadas()).size();
		int fuertesTotales = managerObjetos.getFuertes().size();
		
		if (fuertesTotales-resultadoFinal < resultadoFinal && DetectorAtascamientoD.robotVivo()) {
			System.out.println("Ganamos " + resultadoFinal + " a " + (fuertesTotales-resultadoFinal));
		} else {
			System.out.println("Perdimos " + (fuertesTotales-resultadoFinal) + " a " + resultadoFinal);
		}
		
		try {
			entorno.enviarBeep();
			Thread.sleep(100);
			entorno.enviarBeep();
		} catch (Exception e) {}
		// </BORRAR>
		
		monitorAtascamiento.killThread();
		
		enviador.shutdown();
		enviador.stop();
		atendedor.shutdown();
		atendedor.stop();
		
	}
	
	/**
	 * Ejecuta una tarea generalmente asociada a la colaboración con un 
	 * aliado que previamente coordino la cooperacion
	 * En general va a terminar siendo enviar al robot a defender/conquistar un fuerte
	 * o si no a atacar a un enemgio
	 * @throws IOException 
	 */
	public void ejecutarTarea(Pedido pedidoAyuda) throws IOException {
		synchronized (TERMINE_COOPERAR) {
			TERMINE_COOPERAR = false;
		}
		
		int idObjetivo = pedidoAyuda.getIdObjetivo();
		
		// Parece una estupidez hacer estos if, siendo que todos hacen exactamente lo mismo
		// Estan separado simplemente por claridad, y por si llega que hacer alguna consideracion puntual
		if (pedidoAyuda.esAtacar()) {
			moverRobotHaciaObjetivo(idObjetivo);
		} else if (pedidoAyuda.esConquistar()) {
			moverRobotHaciaObjetivo(idObjetivo);
		} else if (pedidoAyuda.esDefender()) {
			int enemigoAmenzante = deliberador.enemigoAmenzante(entorno.getCoordenadas(idObjetivo)); 
			if (enemigoAmenzante >= 0) {
				moverRobotHaciaObjetivo(enemigoAmenzante);
			} else {
				moverRobotHaciaObjetivo(idObjetivo);
			}
		} else {
			System.err.println("Algun problema hubo aca, porque estoy ejecutando tarea que no es ni atacar ni conquistar ni defender");
		}
		
		synchronized (TERMINE_COOPERAR) {
			TERMINE_COOPERAR = true;
		}
		
	}

	/**
	 * Retorna un Pedido indicando la accion por la cual se desea pedir ayuda 
	 * En el caso de que no se necesario pedir colaboracion, devuelve un pedido 
	 * de tipo cualquiera distinto de Ayuda
	 * @throws IOException 
	 */
	private Pedido pedirAyuda() throws IOException {
		// Por defecto no solicitamos ayuda
		Pedido pedidoAyuda = new Pedido();
		pedidoAyuda.setTipo(Pedido.TipoPedido.IMBUSY);
		if (estoyDefendiendoAtaque) {
			pedidoAyuda = new Pedido(Pedido.TipoPedido.HELPME, Pedido.TipoTarea.ATACAR, enemigoAtacante, "0", 0);
		} else {		
			// Me fijo cuantos fuertes tenemos conquistados al momento
			int sumatoriaConquistasInicial = deliberador.getFuertesConquistables(-1, entorno.getCoordenadas()).size();
			int mejorFuerte = -1;
			
			ArrayList<Integer> fuertes = managerObjetos.getFuertes();
			ArrayList<Integer> aliados = managerObjetos.getAliadosActivos();
			/* Para cada fuerte vamos a determinar la sumatoria de conquistar que resulta de 
			 * cambiar la colocacion de un aliado por la de un fuerte por ejemplo, para ver
			 * que sucede si lo mando hacia ahi, a ver cuantos fuertes estariamos conquistando
			 */
			for (Integer fuerte : fuertes) {
				int sumatoriaConquistas = 0;
				Point3d posHipotetica = entorno.getCoordenadas(fuerte);
				
				for (Integer aliado : aliados) {
					int posiblesConquistas = deliberador.getFuertesConquistables(aliado, posHipotetica).size();
					sumatoriaConquistas += posiblesConquistas;
				}
				
				// Aca verificamos si se logra una mejora con respecto al estado actual
				if (sumatoriaConquistas > sumatoriaConquistasInicial*fuertes.size()) {
					sumatoriaConquistasInicial = sumatoriaConquistas;
					mejorFuerte = fuerte;
				}
				  
			}
			
			if (mejorFuerte >= 0) {
				/* Una vez que tenemos el mejor fuerte, tenemos que ver si conquistarlo o defenderlo
				 * Atacar enemigo no se usa aca, solamente la vamos a usar cuando estamos siendo atacados
				 */
				Point3d posicionMejorFuerte = entorno.getCoordenadas(mejorFuerte);
				int enemigoAmenzante = deliberador.enemigoAmenzante(posicionMejorFuerte);
				if (enemigoAmenzante >= 0) {
					// Hay un enemigo cerca del fuerte, mando a defender el fuerte 
					// TODO PONER BIEN LA IP Y EL PUERTO
					pedidoAyuda = new Pedido(Pedido.TipoPedido.HELPME, Pedido.TipoTarea.DEFENDER, mejorFuerte, null, 0);
				} else {
					pedidoAyuda = new Pedido(Pedido.TipoPedido.HELPME, Pedido.TipoTarea.CONQUISTAR, mejorFuerte, null, 0);
				}
			}
		}
		
		return pedidoAyuda;
	}

	/**
	 * Retorna true si la estrategia determino que algun enemigo activo
	 * me esta atacando
	 */
	private boolean defendiendoAtaque() {
		boolean resultado;
		
		synchronized (estoyDefendiendoAtaque) {
			resultado = estoyDefendiendoAtaque;
		}
		
		return resultado;
	}

	public Pedido obtenerICanOptimo(Atendedor atendedor, Enviador enviador, Point3d posObjetivo) throws IOException {
		Pedido mejorAliado = null;
		
		// Ordeno las distancias de mis aliados de menor a mayor
		List<AliadoDistancia> distancias = new ArrayList<AliadoDistancia>();
		ArrayList<Integer> aliados = managerObjetos.getAliadosActivos();
		Iterator<Integer> iterAliados = aliados.iterator();
		while (iterAliados.hasNext()) {
			int idAliado = iterAliados.next();
			double distancia = utiles.distanciaEuclidea(posObjetivo, entorno.getCoordenadas(idAliado));
			AliadoDistancia ad = new AliadoDistancia(idAliado, distancia);
			distancias.add(ad);
		}
		
		Collections.sort(distancias);
		
		// Recorremos la lista de distancias preguntando si hay algun ican en el buffer para ese aliado
		Iterator<AliadoDistancia> iterDistancias = distancias.iterator();
		boolean encontre = false;
		while (iterDistancias.hasNext() && !encontre) {
			AliadoDistancia ad = iterDistancias.next();
			
			synchronized (atendedor.respuestasAliados) {	
				Iterator<Pedido> iter = atendedor.pedidosAliados.iterator();
				while (iter.hasNext()) {
					Pedido respuesta = iter.next();
					String iprespuesta = respuesta.getDireccionIP();
					// busco el aliado mas cercano que no me haya dicho que esta ocupado
					if (ad.getIdAliado() == enviador.mapearIPAObjeto(iprespuesta) && respuesta.esIcan()) { 
						mejorAliado = respuesta;
						encontre = true;
						break;
					}
				}
			}
			
		}
		
		
		return mejorAliado;
	}
	
	public Pedido obtenerAliadoICanMasCercano(Atendedor atendedor, Enviador enviador, Point3d posObjetivo) throws IOException {
		Pedido mejorAliado = null;
		synchronized (atendedor.respuestasAliados) {
			Iterator<Pedido> iter = atendedor.respuestasAliados.iterator();
			System.err.println("El tamano del buffer de respuestas aliados es " + atendedor.respuestasAliados.size());
			while (iter.hasNext()) {
				Pedido respuesta = iter.next();
				if (respuesta.esIcan()) { 
					if (mejorAliado == null) 
						mejorAliado = respuesta;
					else {
						int idNuevoAliado = enviador.mapearIPAObjeto(respuesta.getDireccionIP());
						int idMejorAliado = enviador.mapearIPAObjeto(mejorAliado.getDireccionIP());
						double distanciaNuevoAliado = utiles.distanciaEuclidea(posObjetivo, entorno.getCoordenadas(idNuevoAliado));
						double distanciaMejorAliado = utiles.distanciaEuclidea(posObjetivo, entorno.getCoordenadas(idMejorAliado));
						if (distanciaMejorAliado > distanciaNuevoAliado)
							mejorAliado = respuesta;
					}
						
				}
			}
		}
		return mejorAliado;	
	}
	
	
	public void ejecutarEstrategia() throws IOException {
		// TODO no se si esta bien reiniciar esta marca aca, o si tendria que hacerlo alguien mas
		estoyDefendiendoAtaque = false;
		
		Point3d miPos = entorno.getCoordenadas();
		// Puede haber un enemigo cerca, acechando, atacando, o simplemente pasando.
		// Para nosotros la cercania se interpreta como amenaza indistintamente
		int enemigoAmenazante = deliberador.enemigoAmenzante(miPos);
		if (enemigoAmenazante >= 0) {
			// Intentamos deducir si esta queriendo atacar, o si simplemente esta cerca
			boolean probableAtaque = predictor.ataqueInminente(enemigoAmenazante);
			if (probableAtaque) {
				synchronized (estoyDefendiendoAtaque) {
					estoyDefendiendoAtaque = true;
					enemigoAtacante = enemigoAmenazante;
				}
				// No hay mejor defensa que un buen ataque
				// Atacamos al enemigo directamente antes de que el nos ataque a nosotros
				moverRobotHaciaObjetivo(enemigoAmenazante);
			}
		} else if (deliberador.voyGanando()) {
			System.out.println("Voy ganando :: " + segundosRestantes());
			// Buscamos un punto del entorno cercano que permita mejorar un poco
			// por ejemplo dismuyendo la suma de distancias a los fuertes conquistados
			Point3d siguientePunto = deliberador.mejorarPuntoOptimo();
			if (siguientePunto != null) {					
				int objetoCercano = deliberador.objetoCercano(siguientePunto);
				// Si hay un objeto cerca del punto destino lo usamos como referencia
				// En cambio si la zona esta momentaneamente libre, vamos al propio punto
				if (objetoCercano < 0) {
					System.out.println("Corrigiendo un poquito");
					// Aca podemos ir con direct nomas, porque asumimos que no hay nadie cerca
					// y el punto esta al ladito, o sea que no deberia haber problemas
					moverRobotDirect(siguientePunto);
				} else {
					System.out.println("Iba a corregir un poquito pero tengo un objeto cerca, voy a el");
					// Mandamos al robot a acercarse al objeto de destino
					moverRobotHaciaObjetivo(objetoCercano);
				}
			} else {
				System.out.println("Voy ganando y no me sirve moverme :: " + segundosRestantes());
				try {
					Thread.sleep(1000);
				} catch (InterruptedException e) {}		
			}
		} else {
			System.out.println("Empatando o perdiendo");
			// Estoy empatando o perdiendo
			ArrayList<Integer> fuertes = managerObjetos.getFuertes();
			// TODO por ahora asumimos que si no estamos ganando estamos perdiendo
			// Elegimos el siguiente punto al cual ir
			Point3d siguientePunto = deliberador.calcularPuntoOptimo(fuertes);
			
			int cantFuertesHipoteticos = deliberador.getFuertesConquistables(-1, siguientePunto).size();
			// Reviso si aun moviendome al punto "optimo" mi bando sigue perdiendo
			if (cantFuertesHipoteticos < fuertes.size()-cantFuertesHipoteticos) {
				// Busco al enemigo que mas fuertes tiene conquistados al momento
				// y trato de atacarlo a el. En este caso con mi posicion actual y no
				// con la elegida, ya que en igualmente sigo perdiendo
				int mejorEnemigo = deliberador.obtenerMejorEnemigo();
				// TODO aca se supone que siempre va a ser un indice valido, porque si no
				// no podria ir perdiendo, pero por las dudas falta ver que hacer en el else
				if (mejorEnemigo >= 0) {
					System.out.println("DECISION: Atacar al objeto " + mejorEnemigo);
					
					// TODO: Derrapa mucho, debe calibrarse
					//Aumentamos nuestra velocidad para atacar!!
					ParametrosEntorno.MAXIMA_VELOCIDAD_DIRECT = ParametrosEntorno.MAXIMA_VELOCIDAD_DIRECT * 1.5;
					ParametrosEntorno.FACTOR_VARIACION_VELOCIDAD_DIRECT = ParametrosEntorno.FACTOR_VARIACION_VELOCIDAD_DIRECT * 1.5;
					
					
					entorno.enviarBeep();
					moverRobotHaciaObjetivo(mejorEnemigo);
					
					// TODO: Derrapa mucho, debe calibrarse
					//Regresamos la velocidad a su estado normal
					ParametrosEntorno.MAXIMA_VELOCIDAD_DIRECT = ParametrosEntorno.MAXIMA_VELOCIDAD_DIRECT / 1.5;
					ParametrosEntorno.FACTOR_VARIACION_VELOCIDAD_DIRECT = ParametrosEntorno.FACTOR_VARIACION_VELOCIDAD_DIRECT / 1.5;
					
					
				} else {
					System.out.println("Aparentemente voy perdiendo pero tenemos todos los fuertes :S");
				}
			} else {
				System.out.print("DECISION: Ir a punto optimo " + utiles.formatearPunto(siguientePunto));
				// Tratamos de ir hacia el punto "optimo"
				moverRobotHaciaObjetivo(siguientePunto);
				try {
					Thread.sleep(200);
				} catch (InterruptedException e) {}		
				
			}				
		}
	
	}
	
	/**
	 * Usa los campos de potenciales para moverse a un punto
	 * @throws IOException 
	 */
	private void moverRobotHaciaObjetivo(Point3d puntoDestino) throws IOException {
		controlCamposPotencial(puntoDestino);
		//moverRobotDirect(puntoDestino);
	}
	
	/**
	 * Usa los campos de potenciales para moverse a un objeto particular
	 * Deberia resolver el problema de detectar si es un fuerte, un enemigo o un aliado
	 * de forma transparente a la estrategia
	 * @throws IOException 
	 */
	private void moverRobotHaciaObjetivo(int objetoDestino) throws IOException {
		controlCamposPotencial(objetoDestino);
		//moverRobotDirect(entorno.getCoordenadas(objetoDestino));
	}
	
}